/*
 * Gets data from the IR sensors and the gyro
 */
#include <xqueue.h>
#include <xtask.h>
#include <rrcon.h>

#include "sensor_read.h"

namespace Group5 {
    using namespace kthrobot;

    SensorsReader::SensorsReader()
        : XTask("SensorsReader",90) {
            SetPeriodMS(period_);
        }

    SensorsReader::~SensorsReader() {
        Join();
    }

    void SensorsReader::TaskCode() {
        while(!MustStop()) {
            new_data_ = true;

            // Raw data
            ADCRawData raw_data = RRCon::GetADCDataSlow();
            data_queue_.Enq(raw_data);

            // Data from the gyroscope
            int angle = raw_to_angle_(raw_data.channel[7]);
            angles_queue_.Enq(angle);
            WaitPeriod();
        }
    }

    int SensorsReader::GetDistanceCm(int index) {
        // Get the distance in centimeters from the IR sensor 'index'
        // Average all values in the queue
        // Returns the distance in cm and -1 in case of an error
        if (index > 5 || index < 0) return -1; // Check the index bounds
        update_averaged_data_();
        return ConvertToCm(averaged_data_.channel[index], index);
    }
         
    int SensorsReader::ConvertToCm(int value, int type) {
        // Convert a raw voltage value to a distance
        // type = 0-3 for short-range IR sensors
        // type = 4-5 for long-range IR sensors
        // Return the value in centimers.
        if (type < 4) {
            return ir_calibration_function_(value, 3.5, 16201, 146);
        }
        else {
            return ir_calibration_function_(value, 1.5, 22675, 32);
        }
    }

    int SensorsReader::ir_calibration_function_(
            int V, int k, int m_prime, int b_prime) {
        // Calibration formula for the IR sensors. V is the voltage value given
        // by the ADC converter. The other arguments are the parameters.
        return (m_prime / (V + b_prime)) - k;
    }

    int SensorsReader::raw_to_angle_(int raw_value) {
        // Calibration function to convert the raw value provided by the gyro
        // to an angle, in 100th of degrees.
        int centered_value = raw_value - gyro_offset_;
        // Using a treshold to remove some noise
        if (centered_value < gyro_treshold_ && centered_value > -gyro_treshold_) {
            return 0;
        }
        return centered_value*period_*1000/gyro_scale_;
    }

    void SensorsReader::update_averaged_data_() {
        // Check if there is new data on the queue
        if (!new_data_) { return; }
        // Averages all data in the queue
        ADCRawData raw_data;
        // Initializing to zero
        for(int i=0; i<=7; i++) {
            averaged_data_.channel[i] = 0;
        }
        // Summing
        new_data_ = false;
        int n;
        for(n=0; data_queue_.Deq(&raw_data); n++) {
            for(int i=0; i<=7; i++) {
                averaged_data_.channel[i] += raw_data.channel[i];
            }
        }
        // Dividing (averaging)
        for(int i=0; i<=7; i++) {
            averaged_data_.channel[i] = averaged_data_.channel[i] / n;
        }
        /*
        // Doing no averaging for testing
        ADCRawData raw_data;
        // Getting the last input
        if (data_queue_.Deq(&raw_data)) {
            while(data_queue_.Deq(&raw_data)) {}
            averaged_data_ = raw_data;
        }
        */
    }


};
